#include <QDebug>
#include "driver.h"
#include "world.h"
#include "streetmap.h"
#include "trafficcontrol.h"
#include "utilities.h"
#include "geometryutilities.h"
#include <qmath.h>

using namespace Utilities;
using namespace GeometryUtilities;

#define CAPTURE_DIST 3.048 // in meters

DriverType::DriverType()
  : preferredTurnSpeed(10.0 * MPH_TO_FTPS * World::gSimTimeStep),
  preferredAcceleration(6.0 * MPH_TO_FTPS * World::gSimTimeStep * World::gSimTimeStep)
{};


Driver::MapPoint::MapPoint(const StreetMap::Intersection *intrSection, const Point &location)
  : intersection(intrSection),
  loc(location)
{}

Driver::Driver(const QString &name)
{
  mName = name;
  mIsCalculatingPath = false;
  mDesiredDirection = 0;
  mHasPath = false;
  mReachedDestination = true;
  mPreferredTurnSpeed = 25;
  mPreferredAcceleration = 1.0 * MPH_TO_FTPS * World::gSimTimeStep * World::gSimTimeStep;
  qDebug() << "DRIVER: spawned - " << mName;
  P =0;
  I =0;
  D =0;
  speedDiff =0;
  mStopping = false;
  mYieldingTo = NULL;
  mPrevSteerVal =0;
  mDesiredDirection = 0;
  mDesiredSpeed = 0;
  mDistToGoal = 0;
  mTimeToGoal = 0;
  mSpeedDelta = 0;
  mHeadingDelta = 0;
  mCrashed = false;
  mDefaultSpeed = 35.0;
  mCurrentRoad = NULL;
  mLastIntersection = NULL;
  mWaypointAdjusted = false;
  mCurrentLaneIndex = 0;
  mPrevSpeedDelta = 0;
  //mCurrentLane = NULL;
  mLastTurnSpeed = 0.0;
  mUpcomingTurn = false;
  mStoppedAtLight = false;
}

Driver::Driver(const QString &name, Vehicle *vehicle)
{
  mName = name;
  mIsCalculatingPath = false;
  mHasPath = false;
  mReachedDestination = true;
  mPreferredTurnSpeed = 25;
  qDebug() << "DRIVER: spawned - " << mName;
  getIn(vehicle);
  P =0;
  I =0;
  D =0;
  speedDiff =0;
  mStopping = false;
  mYieldingTo = NULL;
  mPrevSteerVal =0;
  mDesiredDirection = 0;
  mDesiredSpeed = 0;
  mDistToGoal = 0;
  mTimeToGoal = 0;
  mSpeedDelta = 0;
  mHeadingDelta = 0;
  mDefaultSpeed = 35.0;
  mCurrentRoad = NULL;
  mLastIntersection = NULL;
  mWaypointAdjusted = false;
  mCurrentLaneIndex = 0;
  mPrevSpeedDelta = 0;
  //mCurrentLane = NULL;
  mLastTurnSpeed = 0.0;
  mUpcomingTurn = false;
  mStoppedAtLight = false;
}

void Driver::setName(const QString &value)
{
  mName = value;
}

QString Driver::name()
{
  return mName;
}

void Driver::getIn(Vehicle *vehicle)
{
  mVehicle = vehicle;
  if (mVehicle)
    mVehicle->setDriver(this);
  qDebug() << "DRIVER: " << mName << " got in Vehicle:" << mVehicle->name();
}

void Driver::getIn(const QString &vehicleName)
{
  Vehicle *v = 0;
  World *w = World::instance();
  for (int i=0; i<World::instance()->mDrivers.size(); i++)
    if (w->mVehicles[i]->name() == vehicleName)
      v = w->mVehicles[i];
  if (v)
    getIn(v);
  else
    qDebug() << "DRIVER: ERROR - Could not find Vehicle: " << vehicleName;
}

void Driver::getOut()
{
  qDebug() << "DRIVER: " << mName << " got out of Vehicle:" << mVehicle->name();
  mVehicle = 0;
}

Point Driver::destination()
{
  return (mPath.isEmpty()) ? Point() : mPath.last().loc;
}

void Driver::addWaypoint(const Point &p)
{
  mPath.push_back(MapPoint(NULL,p));
  mDestination = p;
  //qDebug() << "DRIVER: " << mName << " addWaypoint=" << p.toString();
}

void Driver::addWaypoint(const QString &intersectionName)
{
  World *w = World::instance();
  const StreetMap::Intersection *i = w->mStreetMap->getIntersection(intersectionName);
  if (i)
  {
    mPath.push_back(MapPoint(i, i->location()));
    mDestination = i->location();
    //qDebug() << "DRIVER: " << mName << " addWaypoint=" << i->name() << ":" << i->location().toString();
  }
  else
  {
    qDebug() << "DRIVER: " << mName << "ERROR> Waypoint" << intersectionName << "not found.";
  }
}

void Driver::calculatePath()
{
  qDebug() << "DRIVER: Calculating path to: " << mDestination.toString();

  // A* search here? Or predetermined path?
  // Path selection heuristics applied here.
  mHasPath = true;
}

bool Driver::driveToGoal(const Point &goal, const GoalType goalType)
{
  if (!mVehicle || mReachedDestination)
   return false;

  bool ret = false;
  mCurrentGoal = goal;
  qreal speed = mVehicle->speed();
  mDistToGoal = (mCurrentGoal - mVehicle->position()).magnitude() * METER_PER_FEET;
  mTimeToGoal = mDistToGoal/speed;
  //qreal distToStop = mVehicle->distUntilSpeed(0.0);
  //qreal timeToStop = mVehicle->timeUntilSpeed(0.0);
  mDesiredDirection = Point::bearing(mVehicle->position(), mCurrentGoal);
  qreal urgency = 2.0;

  if (goalType == GOAL_WAYPOINT)
  {
    // Check if I should just maintain last speed
    if (mLastTurnSpeed > 0.0)
    {
      mDesiredSpeed = mLastTurnSpeed;
      //qDebug() << "1.1 - Maintain last! desspd=" << mDesiredSpeed;
    }
    else //...Re-evaluate my speed
    {
      // Figure out if I'll be turning when I get to this waypoint
      Point next;
      if (mPath.size()==1)
      {
        // Coming to destination, slow to turn speed anyway
        mUpcomingTurn = true;
      }
      else //...not coming to destination
      {
        next = mPath.at(1).loc;
        qreal turnBearing = Point::bearing(mCurrentGoal, next);
        qreal bearingDelta = qAbs(Point::clampAngleTo180(turnBearing - mVehicle->direction().angle()));
        mUpcomingTurn = (bearingDelta > 20.0);
      }

      // Check if I'm getting close to my waypoint
      qreal distToPrefTurnSpeed = mVehicle->distUntilSpeed(mPreferredTurnSpeed);
      if (mDistToGoal < (30.0*METER_PER_FEET))
      {
        mDesiredSpeed = (mUpcomingTurn) ? mPreferredTurnSpeed : mDefaultSpeed;
        mLastTurnSpeed = (mUpcomingTurn) ? mPreferredTurnSpeed : mDefaultSpeed;
        //qDebug() << "1.2 - WAYPOINT less than 30ft! desspd=" << mDesiredSpeed;
      }
      else if (mDistToGoal <= distToPrefTurnSpeed)
      {
        mDesiredSpeed = (mUpcomingTurn) ? mPreferredTurnSpeed : mDefaultSpeed;
        mLastTurnSpeed = (mUpcomingTurn) ? mPreferredTurnSpeed : mDefaultSpeed;
        //qDebug() << "1.2 - WAYPOINT decide time! desspd=" << mDesiredSpeed;
      }
      else //... Not close, so just cruise to waypoint, normal speed
      {
        mDesiredSpeed = mDefaultSpeed;
        //qDebug() << "1.3 - WAYPOINT cruise! desspd=" << mDesiredSpeed;
      }
    }
  }
  else if (goalType == GOAL_OBSTACLE) // Approaching an obstacle, (not a waypoint)
  {
    //qDebug() << "5.0 tts=" << timeToStop << "ttg=" << mTimeToGoal;
if (name() == "Driver6") qDebug() << "OBSTACLE!";
    qreal distToStop = mVehicle->distUntilSpeed(0.0);
    // 10== Prefferred spacing
    if (mDistToGoal < (15.0*METER_PER_FEET))
    {
      mDesiredSpeed = 0.0;
      urgency = 100.0;
      //mDesiredDirection = mVehicle->direction().angle();
      if (name() == "Driver6") qDebug() << "5.1 OBSTACLE Less than 10ft! desspd=" << speed;
    }
    else if (mDistToGoal <= distToStop)
    {
      mDesiredSpeed = 5.0;
      //mDesiredDirection = mVehicle->direction().angle();
      if (name() == "Driver6") qDebug() << "5.1 OBSTACLE decide desspd=" << mDesiredSpeed;
    }
    else
    {
      if (name() == "Driver6") qDebug() << "5.2 OBSTACLE cruise! desspd=" << mDesiredSpeed;
      mDesiredSpeed = mDefaultSpeed;
    }
    //qDebug() << "5.9 - OBSTACLE sighted! desspd=" << mDesiredSpeed;
  }
  else if (goalType == GOAL_LANE)// Approaching an obstacle, (not a waypoint)
  {
    // I'll start acceleration as I emerge from turn
    // NOTE: Is this math right for all angle combinations?
    qreal angleDelta = qAbs(Point::clampAngleTo180(mDesiredDirection - mVehicle->direction().angle()));
    mDesiredSpeed = (angleDelta > 20.0) ? mPreferredTurnSpeed : mDefaultSpeed;
    //qDebug() << "4 - Drive to LANE! desspd=" << mDesiredSpeed;
  }

  // Drive the car
  drive(urgency);
  return ret;
}

void Driver::drive(float urgency)
{
  //qDebug() << "Drive...";
  if (mVehicle)
  {
    float speed = mVehicle->speed(true);
    float accelVal = 0.0, brakeVal = 0.0;
    float speedDelta = 0.0f;
    speedDelta = mDesiredSpeed - speed;
    mSpeedDelta = speedDelta;

    // NOTE: Make the 3.0f and the 2.0f adjustable. 2.0=urgency?
    if (speedDelta < -3.0f)
    {
      accelVal = 0.0;
      brakeVal = qAbs(speedDelta * urgency / 100.0f);
    }
    else if (speedDelta > 3.0f)
    {
      accelVal = qAbs(speedDelta * urgency / 100.0f);
      brakeVal = 0.0;
    }
    else if (mDesiredSpeed==0.0)
    {
      accelVal = 0.0;
      brakeVal = 1.0;
    }

    // Steering
    float angleDelta = mDesiredDirection - mVehicle->direction().angle();
    float steerVal = 0.0f;
    angleDelta = Point::clampAngleTo180(angleDelta);
    mHeadingDelta = angleDelta;
    steerVal = 2 * (angleDelta) / 180.0;
    qreal steerChange=0;
    steerChange = steerVal - mPrevSteerVal;
    if(steerChange > .2)
      steerChange = .2;
    else if(steerChange < -.2)
      steerChange = -.2;
    steerVal = mPrevSteerVal + steerChange;

    if(steerVal > 1)
      steerVal = 1;
    else if(steerVal < -1)
      steerVal = -1;
    mPrevSteerVal = steerVal;

    mVehicle->accelerate(accelVal);
    mVehicle->brake(brakeVal);
    mVehicle->steer(steerVal);
  }
}

bool Driver::checkCollisions(Point &collision)
{
  World *w = World::instance();
  Point p, collisionPoint;
  bool willCollide = false;
  qreal shortestDistance=10000.0f;

  Point frontOfVeh = mVehicle->position()+(mVehicle->direction()*mVehicle->wheelBase());
  //Point frontOfVeh = mVehicle->position();

  Ray r3(mVehicle->position(), mVehicle->direction()); // current dir
  Ray r1(frontOfVeh, mCurrentWaypoint-frontOfVeh); // dir to waypoint
  mYieldingTo = NULL;

  foreach (Driver *d, w->mDrivers)
  {
    if (d && d != this)
    {
      Vehicle *v = d->mVehicle;
      IntersectionInfo inf = NO_INTERSECTION;

      // Check if any cars are between me and my waypoint
      QRect rect(0,0,20,20);//wheelbase?
      rect.moveCenter(v->position().toPoint());
      Point pts[2];

      RayRectIntersect(r1, rect, pts, inf);
      if (inf == INTERSECTION)
      {
        qreal distToCollision = (frontOfVeh-pts[0]).magnitude();
        if (distToCollision <= 150.0) // driver's lookahead distance
        {
          if (distToCollision < shortestDistance)
          {
            if (d->mYieldingTo != this)
            {
              mYieldingTo = d;
              collision = pts[0];
              shortestDistance = distToCollision;
              willCollide = true;
            }
          }
        }
      }


      //Check if any cars are directly in front of me
      RayRectIntersect(r3, rect, pts, inf);
      if (inf == INTERSECTION)
      {
        qreal distToCollision = (frontOfVeh-pts[0]).magnitude();
        if (distToCollision <= 150.0) // driver's lookahead distance
        {
          if (distToCollision < shortestDistance)
          {
            if (d->mYieldingTo != this)
            {
              mYieldingTo = d;
              collision = pts[0];
              shortestDistance = distToCollision;
              willCollide = true;
            }
          }
        }
      }

      // Anticipate possible collisions ahead of time
      Ray r2(v->position(), v->direction());
      collisionPoint = RayRayIntersect(r1, r2, inf);
      if (inf == INTERSECTION)
      {
        qreal myDistToCollision = (frontOfVeh-collisionPoint).magnitude() * METER_PER_FEET;
        qreal theirDistToCollision = (v->position()-collisionPoint).magnitude()* METER_PER_FEET;
        qreal mySpeed = mVehicle->speed(), theirSpeed = v->speed();
        qreal myTimeToCollide=0.0, theirTimeToCollide=0.0;
        qreal shortestTime = 90000.0;
        myTimeToCollide = (mySpeed > 0.0) ? myDistToCollision/mySpeed : 90000.0;
        theirTimeToCollide = (theirSpeed > 0.0) ? theirDistToCollision/theirSpeed : 10000.0;
        qreal timeDelta = qAbs(myTimeToCollide-theirTimeToCollide);

        //qDebug() << "Driver:" << name() << "crash with" << d->name() << "mt=" << myTimeToCollide << "tt=" << theirTimeToCollide << "d=" <<timeDelta;
        if (myTimeToCollide <= 2 && timeDelta <= 2)
        {
          if (myDistToCollision < shortestDistance)
          {
            if(timeDelta < shortestTime)
            {
              //if (d->mYieldingTo != this && theirTimeToCollide < myTimeToCollide)
              if (d->mYieldingTo != this && theirDistToCollision < myDistToCollision)
              {
                mYieldingTo = d;
                collision = collisionPoint;
                shortestDistance = myDistToCollision;
                willCollide = true;
                shortestTime = timeDelta;
              }
            }
          }
        }
      }

    }
  }

  return willCollide;
}

bool Driver::checkLane(Point &laneGoal)
{
  laneGoal = mCurrentLane.segment.closestPointOn(mVehicle->position());
  qreal dist = (laneGoal-mVehicle->position()).magnitude();
  if (dist >= LANE_WIDTH/2)
  {
    Point v = (mCurrentWaypoint-laneGoal).normalized();
    laneGoal = laneGoal + (v * 5.0);
    return true;
  }
  return false;
}

bool Driver::checkTrafficControls(Point &controlPoint)
{
  if (!mCurrentRoad || mPath.size()<2)
    return false;

  World *w = World::instance();
  StreetMap *map = w->mStreetMap;

  TrafficControl *ct = w->mTrafficControls.value(mPath.at(0).intersection);
  if (ct)
  {
    TrafficControl::LIGHT_COLOR c = ct->currentState()->lightColor(
        mCurrentRoad, mCurrentLaneIndex,
        map->getRoad(mPath.at(0).intersection, mPath.at(1).intersection), mCurrentLaneIndex);
/*
    if (c==TrafficControl::LIGHT_GREEN)
      qDebug() << "SIGNAL STATE: GREEN";
    else if (c==TrafficControl::LIGHT_YELLOW)
      qDebug() << "SIGNAL STATE: YELLOW";
    else if (c==TrafficControl::LIGHT_RED)
      qDebug() << "SIGNAL STATE: RED";
*/
    if (c==TrafficControl::LIGHT_YELLOW || c==TrafficControl::LIGHT_RED)
    {

      // Set current waypoint back from lane end 20ft
      Point v = mCurrentLane.segment.closestPointOn(mVehicle->position())-mCurrentWaypoint;
      v.normalize();
      //controlPoint = mCurrentWaypoint + (v* 20.0f);
      controlPoint = mCurrentWaypoint;
      return true;
    }


  }
  return false;

}


void Driver::adjustWaypoint()
{
  World *w = World::instance();
  StreetMap *map = w->mStreetMap;

  // Get the path intersection I'm heading for
  const StreetMap::Intersection *isec = mPath.at(0).intersection;

  // I'm not actually going towards an official intersection (coordinate specified)
  if (!isec)
  {
    mCurrentWaypoint = mPath.at(0).loc;
    mWaypointAdjusted = true;
    mLastIntersection = NULL;
    return;
  }

  // Figure out what road I'm on, based on my last intersection and my target intersection
  if (!mLastIntersection)
    mCurrentRoad = map->getRoad(mVehicle->position());
  else
    mCurrentRoad = map->getRoad(mLastIntersection, isec);

  if (!mCurrentRoad)
    return;

  StreetMap::Road::Lane::DIRECTION dir = mCurrentRoad->getDirection(isec);
  //unsigned int laneCount = mCurrentRoad->numLanes(dir);

  // Get a reference to the lane I'm trying to drive down
  //mCurrentLaneIndex = laneCount - 1; // prefer outer-most lane (0=inner-most lane)
  mCurrentLaneIndex = 0; // prefer outer-most lane (0=inner-most lane)

  bool valid = false;
  mCurrentLane = mCurrentRoad->lane(dir, mCurrentLaneIndex, valid);
  if (!valid)
    return;

  // Adjust my waypoint to be the end of the lane (in the direction I'm going)
  if (dir == StreetMap::Road::Lane::FROM_FIRST_TO_SECOND)
    mCurrentWaypoint = mCurrentLane.segment.begin();
  else
    mCurrentWaypoint = mCurrentLane.segment.end();

  // Set current waypoint back from lane end
  Point v = mCurrentLane.segment.closestPointOn(mVehicle->position())-mCurrentWaypoint;
  v.normalize();
  mCurrentWaypoint = mCurrentWaypoint + (v*5.0f);

  // Mark that waypoint has been adjusted
  mWaypointAdjusted = true;
}


void Driver::tick()
{
  mReachedDestination = mPath.isEmpty();
  if (!mReachedDestination)
  {
    Point collidePoint;
    Point controlPoint;
    Point lanePoint;
    bool willCollide = false;
    bool notInLane = false;
    bool trafficControl = false;
    qreal distToCollide = 999999.0f;
    qreal distToLane = 999999.0;
    qreal distToTrafficControl = 999999.0;

    if (!mWaypointAdjusted)
      adjustWaypoint();

    qreal distToWpt = (mCurrentWaypoint - mVehicle->position()).magnitude() * METER_PER_FEET;
    if (distToWpt <= CAPTURE_DIST && !mStoppedAtLight)
    {
      qDebug() << "DRIVER:" << name() << "tick> waypoint achieved="
        << mCurrentGoal.toString()
        << " distance=" << mVehicle->travelDistance(true) << "mi"
        << " time=" << World::gSimTime.toString("hh:mm:ss.zzz");
      mLastIntersection = mPath.at(0).intersection;
      mPath.pop_front();
      mWaypointAdjusted = false;
      mLastTurnSpeed = 0.0;
      mUpcomingTurn = false;
      return;
    }

    // Check collisions with other cars
    if (checkCollisions(collidePoint))
    {
      willCollide = true;
      Point towardVehicle = mVehicle->position() - collidePoint;
      distToCollide = towardVehicle.magnitude();
      towardVehicle.normalize();

      //if (distToCollide < 4.0f)
        //qDebug() << "Driver" << name() << "CRASHED!!! Location:" << collidePoint.toString() << " d=" << distToCollide;

      // Move the collide location towards my car a little, so I have
      // some space to stop
      if (distToCollide >= 10.0f)
        collidePoint =  collidePoint + (towardVehicle * 10.0f);
      else
        collidePoint = mVehicle->position();

      // Recalculate distance to collide point after above shift
      // (Maybe just subtract instead?)
      distToCollide = (mVehicle->position() - collidePoint).magnitude();
    }

    // Check if I'm in the lane we want to be in
    if (mPath.at(0).intersection && checkLane(lanePoint))
    {
      notInLane = true;
      distToLane = (mVehicle->position() - lanePoint).magnitude();
    }

    // Check if I'm heading towards a traffic control at the next intersection
    if (checkTrafficControls(controlPoint))
    {
      trafficControl = true;
      mStoppedAtLight = true;
      distToTrafficControl = (mVehicle->position() - controlPoint).magnitude();
    }
    else
      mStoppedAtLight = false;

    // Prioritize drive decision
    if (willCollide && distToCollide < distToLane && distToCollide <= distToWpt+10.0f)
      driveToGoal(collidePoint, GOAL_OBSTACLE);
    else if (notInLane && distToLane < distToCollide)
      driveToGoal(lanePoint, GOAL_LANE);
    else if (trafficControl)
      driveToGoal(controlPoint, GOAL_OBSTACLE);
    else
      driveToGoal(mCurrentWaypoint, GOAL_WAYPOINT);

  }
  else //...I am at my destination, stop
  {
    mDesiredSpeed = 0.0;
    drive(2.0f); // needed so brake is held & doesn't reverse from brake
  }
}
